#include "measuretask.h"
#include "delay.h"
#include "led.h"
#include "AS5600.h"

#include "fast_calculate.h"
#include "foc_motor.h"
#include "PID.h"

#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "event_groups.h"

StateObject _sMotorState;

void v_magnetic_senser_init(void)
{
    v_get_prev_angle();

    _sMotorState.fAngle = 0;
    _sMotorState.fSpeed = 0;
}

float f_get_attitude_angle(void)
{
    return _sMotorState.fAngle;
}

float f_get_attitude_speed(void)
{
    return -_sMotorState.fSpeed;
}

void v_motor_attitude_task(void *param)
{
    uint32_t uiNowUs;
    uint32_t uiLastUs;

    float fAngle;
    float fSpeed;
    float fPrevAngle;
    while(1)
    {
        uiNowUs = xTaskGetTickCount();
        fAngle = f_read_angel();
        fSpeed = (fPrevAngle - fAngle)/(uiNowUs - uiLastUs);
        fPrevAngle = fAngle;

        _sMotorState.fSpeed = _sMotorConfig.iSensorDirection * f_LPF_velocity(fSpeed)*100;
        _sMotorState.fAngle = _sMotorConfig.iSensorDirection * fAngle;
        uiLastUs = uiNowUs;

        vTaskDelay(1);
    }
}




